/*
 * my_resampler.c
 *
 *  Created on: 19 août 2011
 *      Author: cedric
 */

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include "MyReferenceResampler.h"
#include "common.h"

//Struct holding member attributes
struct my_reference_resampler {
	void (*callback)(sample_t s);
	uint32_t M; //window width
	uint32_t channels;
	uint32_t input_rate;
	uint32_t output_rate;
	uint32_t lcm;
	sample_t *chain; //delay line
	window_t *window; //window coeffs
	uint32_t chain_state;
	uint32_t window_state;
	uint32_t output_state;
	uint32_t output_counter;
	uint32_t output_delay;
	uint32_t input_delay;
};

//public methods

MyReferenceResampler my_reference_resampler_new(uint32_t ntaps,
		uint32_t channels, uint32_t input_rate, uint32_t output_rate,
		my_resampler_window_function func, void(*callback)(sample_t s)) {

	//initialize object
	MyReferenceResampler rs = (MyReferenceResampler) malloc(
			sizeof(struct my_reference_resampler));
	rs->input_rate = input_rate;
	rs->output_rate = output_rate;
	rs->channels = channels;
	rs->callback = callback;
	rs->output_state = 0;
	rs->output_counter = 0;

	//compute least common multiplicator between input and output samplerate
	rs->lcm = lcm(input_rate, output_rate);
	uint32_t output_taps = rs->lcm / rs->output_rate;
	uint32_t input_taps = rs->lcm / rs->input_rate;

	rs->M = ntaps;

	rs->output_delay = rs->M / output_taps / 2;
	rs->input_delay = rs->M / input_taps / 2;

	//TODO: M should be a multiple of the scm

	//the cutoff frequency for the lowpass filter is based on the lowest input/output samplerate
	uint32_t cutoff_freq = 2 * rs->lcm
			/ (input_rate <= output_rate ? input_rate : output_rate);

	puts("reference_version");
	printf(
			"Resampler initialized with input rate:%d Hz, output rate:%d Hz, window length:%d, lcm:%d, cutoff_freq:%d \n",
			rs->input_rate, rs->output_rate, rs->M, rs->lcm, cutoff_freq);

	//init window values
	uint64_t i;

	//allocate and initialize M+1 values for the window

	window_t (*win_func)(int32_t i, uint32_t cutoff_freq, uint32_t M);

	switch (func) {
	case WINDOW_NONE:
		win_func = &sinc_none;
		break;
	case WINDOW_BLACKMAN:
		win_func = &sinc_blackman;
		break;
	case WINDOW_KAISER:
		win_func = &sinc_none;
		break;
	default:
		win_func = &sinc_none;
		break;
	}

	//puts("window start:");
	rs->window = malloc((rs->M + 1) * sizeof(window_t));
	for (i = 0; i <= rs->M; i++) {
		rs->window[i] = win_func((i - rs->M / 2), cutoff_freq, rs->M);
	}
		//puts("window end");

		//init chain of values to be processed
	rs->chain = calloc((rs->M+1) * rs->channels, sizeof(sample_t));

	return rs;
}

void my_reference_resampler_destroy(MyReferenceResampler rs) {
	uint32_t i;
	sample_t* s = calloc(rs->channels,sizeof(sample_t));
	for (i = 0; i < rs->input_delay; i++) {
		my_reference_resampler_process_sample(rs, s);
	}
	free(rs->window);
	free(rs->chain);
	free(rs);
}

void my_reference_resampler_process_sample(MyReferenceResampler rs, sample_t* s) {
	//number of intermediary frames per input frame
	uint32_t input_taps = rs->lcm / rs->input_rate;
	//number of intermediary frames per output frame
	uint32_t output_taps = rs->lcm / rs->output_rate;

	uint32_t i;

	//store input at the beginning of the delay line
	//printf ("storing value: %.3f\n", s);
	for (i = 0; i < rs->channels; i++) {
		rs->chain[i] = s[i];
	}

	sample_t accum;

	uint32_t j, k;

	for (i = 0; i < input_taps; i++) {

		if (rs->output_state % output_taps == 0) {

			puts("processing chain/window values:");
			 for (j = 0; j < (rs->M+1)*rs->channels; j++) printf ("%.3f ", rs->chain[j]);
			 puts("");
			 for (j = 0; j < rs->M +1; j++) printf ("%.3f ", rs->window[j]);
			 puts("");


			//compute FIR for each channel
			for (k = 0; k < rs->channels; k++) {
				printf("computing channel %d\n", k);

				accum = 0;
				for (j = 0; j <= rs->M; j++) {
					accum += rs->chain[k+j*rs->channels] * rs->window[j];
					printf("(%.3f)*(%.3f)\n", rs->chain[k+j*rs->channels], rs->window[j]);
				}

				if (rs->output_counter > rs->output_delay*rs->channels) { //don't write first results
					rs->callback(accum);
				} else {
					rs->output_counter++;
				}
				rs->output_state = 0;
			}

		}

		// shift delay line by 1 frame
		for (j = (rs->M+1) * rs->channels -1 ; j >= rs->channels; j--) {
			rs->chain[j] = rs->chain[j - rs->channels];
		}
		for (j=0; j<rs->channels;j++){
			rs->chain[j] = 0;
		}

		rs->output_state++;

	}

}

